#include "mpu6050.h"
#include <math.h>

static float MPU6050_getAccelScale(MPU6050* mpu);

// 获取陀螺仪量程对应的 LSB/dps 比例因子
static float MPU6050_getGyroScale(MPU6050* mpu);

// 内联函数：读寄存器
uint8_t MPU6050_readReg(MPU6050* mpu, uint8_t reg) {
    Wire_beginTransmission(mpu->wire, mpu->addr);
    Wire_write(mpu->wire, reg);
    if (Wire_endTransmission(mpu->wire, 0) != 0) {
        return 0;
    }

    if (Wire_requestFrom(mpu->wire, mpu->addr, 1) != 1) {
        return 0;
    }

    return Wire_read(mpu->wire);
}

// 写寄存器
int MPU6050_writeReg(MPU6050* mpu, uint8_t reg, uint8_t data) {
    Wire_beginTransmission(mpu->wire, mpu->addr);
    Wire_write(mpu->wire, reg);
    Wire_write(mpu->wire, data);
    return Wire_endTransmission(mpu->wire, 1);
}

// 初始化 MPU6050
int MPU6050_init(MPU6050* mpu, Wire* wire, uint8_t address) {
    mpu->wire = wire;
    mpu->addr = address;
    mpu->gyro_fs = MPU6050_GYRO_FS_250DPS;
    mpu->accel_fs = MPU6050_ACCEL_FS_2G;

    // 检查设备是否存在
    uint8_t devid = MPU6050_readReg(mpu, MPU6050_REG_WHOAMI);
    if (devid != 0x68) {
        return -1;  // 不是 MPU6050
    }

    // 唤醒设备，关闭睡眠模式
    MPU6050_writeReg(mpu, MPU6050_REG_PWR_MGMT_1, 0x00);
    MPU6050_writeReg(mpu, MPU6050_REG_USER_CTRL, 0x00);

    // 关闭中断
    MPU6050_writeReg(mpu, MPU6050_REG_INT_ENABLE, 0x00);
    // 旁路模式（让主I2C直接访问外部传感器）
    MPU6050_writeReg(mpu, MPU6050_REG_INT_PIN_CFG, 0x02);

    // 设置默认配置
    MPU6050_writeReg(mpu, MPU6050_REG_CONFIG, 0x03);  // 1kHz 采样率，20Hz 低通滤波
    MPU6050_setGyroFSScale(mpu, MPU6050_GYRO_FS_250DPS);
    MPU6050_setAccelFSScale(mpu, MPU6050_ACCEL_FS_4G);
	
	// 清零偏移
	mpu->accel_offset_x = 0;
	mpu->accel_offset_y = 0;
	mpu->accel_offset_z = 0;
	mpu->gyro_offset_x = 0;
	mpu->gyro_offset_y = 0;
	mpu->gyro_offset_z = 0;
	mpu->calibrated = 0;

    return 0;
}

// 兼容旧名
int MPU6050_begin(MPU6050* mpu, Wire* wire, uint8_t address) {
    return MPU6050_init(mpu, wire, address);
}

// 设置陀螺仪量程
int MPU6050_setGyroFSScale(MPU6050* mpu, uint8_t fs_sel) {
    if (fs_sel > 3) return -1;
    mpu->gyro_fs = fs_sel;
    return MPU6050_writeReg(mpu, MPU6050_REG_GYRO_CONFIG, fs_sel << 3);
}

// 设置加速度计量程
int MPU6050_setAccelFSScale(MPU6050* mpu, uint8_t fs_sel) {
    if (fs_sel > 3) return -1;
    mpu->accel_fs = fs_sel;
    return MPU6050_writeReg(mpu, MPU6050_REG_ACCEL_CONFIG, fs_sel << 3);
}

// 读取原始数据（加速度、角速度、温度）
int MPU6050_readRaw(MPU6050* mpu) {
    Wire_beginTransmission(mpu->wire, mpu->addr);
    Wire_write(mpu->wire, MPU6050_REG_ACCEL_XOUT_H);
    if (Wire_endTransmission(mpu->wire, 0) != 0) {
        return -1;
    }

    if (Wire_requestFrom(mpu->wire, mpu->addr, 14) != 14) {
        return -1;
    }

    uint8_t buf[14];
    for (int i = 0; i < 14; i++) {
        buf[i] = Wire_read(mpu->wire);
    }

    // 解析数据（大端）
    mpu->ax = (int16_t)(buf[0] << 8 | buf[1]);
    mpu->ay = (int16_t)(buf[2] << 8 | buf[3]);
    mpu->az = (int16_t)(buf[4] << 8 | buf[5]);
    mpu->temp = (int16_t)(buf[6] << 8 | buf[7]);
    mpu->gx = (int16_t)(buf[8] << 8 | buf[9]);
    mpu->gy = (int16_t)(buf[10] << 8 | buf[11]);
    mpu->gz = (int16_t)(buf[12] << 8 | buf[13]);

    return 0;
}

// 读取温度（原始值）
int MPU6050_readTemp(MPU6050* mpu) {
    if (MPU6050_readRaw(mpu) != 0) {
        return -1;
    }
    return 0;
}

// 获取摄氏温度
float MPU6050_getTemp_C(MPU6050* mpu) {
    return (mpu->temp / MPU6050_TEMP_SENSITIVITY) + MPU6050_TEMP_OFFSET;
}

// 获取加速度（单位：g）
float MPU6050_getAx_g(MPU6050* mpu) {
    float scale = MPU6050_getAccelScale(mpu);
    if (isnan(scale)) return NAN;
    return (mpu->ax - mpu->accel_offset_x) / scale;
}

float MPU6050_getAy_g(MPU6050* mpu) {
    float scale = MPU6050_getAccelScale(mpu);
    if (isnan(scale)) return NAN;
    return (mpu->ay - mpu->accel_offset_y) / scale;
}

float MPU6050_getAz_g(MPU6050* mpu) {
    float scale = MPU6050_getAccelScale(mpu);
    if (isnan(scale)) return NAN;
    return (mpu->az - mpu->accel_offset_z) / scale;
}

// 获取角速度（单位：dps）
float MPU6050_getGx_dps(MPU6050* mpu) {
    float scale = MPU6050_getGyroScale(mpu);
    if (isnan(scale)) return NAN;
    return (mpu->gx - mpu->gyro_offset_x) / scale;
}

float MPU6050_getGy_dps(MPU6050* mpu) {
    float scale = MPU6050_getGyroScale(mpu);
    if (isnan(scale)) return NAN;
    return (mpu->gy - mpu->gyro_offset_y) / scale;
}

float MPU6050_getGz_dps(MPU6050* mpu) {
    float scale = MPU6050_getGyroScale(mpu);
    if (isnan(scale)) return NAN;
    return (mpu->gz - mpu->gyro_offset_z) / scale;
}

#include "delay.h"
/**
 * @brief 静态校准加速度计（请确保 MPU6050 平放静止）
 */
int MPU6050_calibrateAccel(MPU6050* mpu) {
    int32_t ax_sum = 0, ay_sum = 0, az_sum = 0;
    const int samples = 100;

    for (int i = 0; i < samples; i++) {
        if (MPU6050_readRaw(mpu) != 0) return -1;
        ax_sum += mpu->ax;
        ay_sum += mpu->ay;
        az_sum += mpu->az;
        delay_ms(10);
    }

    mpu->accel_offset_x = ax_sum / samples;
    mpu->accel_offset_y = ay_sum / samples;

    // 根据当前量程计算 1g 对应的 LSB
    int16_t g_value = 16384;
    switch (mpu->accel_fs) {
        case MPU6050_ACCEL_FS_2G:  g_value = 16384; break;
        case MPU6050_ACCEL_FS_4G:  g_value = 8192;  break;
        case MPU6050_ACCEL_FS_8G:  g_value = 4096;  break;
        case MPU6050_ACCEL_FS_16G: g_value = 2048;  break;
    }

    mpu->accel_offset_z = az_sum / samples - g_value;  // 偏移 = 实际 - 理想

    mpu->calibrated = 1;
    return 0;
}

/**
 * @brief 静态校准陀螺仪（请确保 MPU6050 完全静止）
 */
int MPU6050_calibrateGyro(MPU6050* mpu) {
    int32_t gx_sum = 0, gy_sum = 0, gz_sum = 0;
    const int samples = 100;

    for (int i = 0; i < samples; i++) {
        if (MPU6050_readRaw(mpu) != 0) {
            return -1;
        }
        gx_sum += mpu->gx;
        gy_sum += mpu->gy;
        gz_sum += mpu->gz;
        delay_ms(10);
    }

    mpu->gyro_offset_x = gx_sum / samples;
    mpu->gyro_offset_y = gy_sum / samples;
    mpu->gyro_offset_z = gz_sum / samples;

    mpu->calibrated = 1;
    return 0;
}

int MPU6050_getMotion(
    MPU6050* mpu,
    float* ax_g, float* ay_g, float* az_g,
    float* gx_dps, float* gy_dps, float* gz_dps,
    float* temp_c
) {
    if (MPU6050_readRaw(mpu) != 0) {
        return -1;
    }

    // 加速度（带校准）
    float accel_scale = 1.0f;
    switch (mpu->accel_fs) {
        case MPU6050_ACCEL_FS_2G:  accel_scale = 16384.0f; break;
        case MPU6050_ACCEL_FS_4G:  accel_scale = 8192.0f;  break;
        case MPU6050_ACCEL_FS_8G:  accel_scale = 4096.0f;  break;
        case MPU6050_ACCEL_FS_16G: accel_scale = 2048.0f;  break;
        default: accel_scale = 16384.0f; break;
    }
	
	int32_t ax_raw = mpu->ax - mpu->accel_offset_x;
	int32_t ay_raw = mpu->ay - mpu->accel_offset_y;
	int32_t az_raw = mpu->az - mpu->accel_offset_z;

	*ax_g = ax_raw / accel_scale;
	*ay_g = ay_raw / accel_scale;
	*az_g = az_raw / accel_scale;  // 自动包含 1g 重力！

    // 角速度（带校准）
    float gyro_scale = 1.0f;
    switch (mpu->gyro_fs) {
        case MPU6050_GYRO_FS_250DPS:  gyro_scale = 131.0f;  break;
        case MPU6050_GYRO_FS_500DPS:  gyro_scale = 65.5f;   break;
        case MPU6050_GYRO_FS_1000DPS: gyro_scale = 32.8f;   break;
        case MPU6050_GYRO_FS_2000DPS: gyro_scale = 16.4f;   break;
        default: gyro_scale = 131.0f; break;
    }

    *gx_dps = (mpu->gx - mpu->gyro_offset_x) / gyro_scale;
    *gy_dps = (mpu->gy - mpu->gyro_offset_y) / gyro_scale;
    *gz_dps = (mpu->gz - mpu->gyro_offset_z) / gyro_scale;

    // 温度
    *temp_c = MPU6050_getTemp_C(mpu);

    return 0;
}

float MPU6050_getTotalAccel(MPU6050* mpu) {
    if (MPU6050_readRaw(mpu) != 0) {
        return NAN;  // 读取失败
    }

    float accel_scale = 1.0f;
    switch (mpu->accel_fs) {
        case MPU6050_ACCEL_FS_2G:  accel_scale = 16384.0f; break;
        case MPU6050_ACCEL_FS_4G:  accel_scale = 8192.0f;  break;
        case MPU6050_ACCEL_FS_8G:  accel_scale = 4096.0f;  break;
        case MPU6050_ACCEL_FS_16G: accel_scale = 2048.0f;  break;
        default: return NAN;
    }

    // 补偿偏移（和 getMotion 一致）
    float ax_g = (mpu->ax - mpu->accel_offset_x) / accel_scale;
    float ay_g = (mpu->ay - mpu->accel_offset_y) / accel_scale;
    float az_g = (mpu->az - mpu->accel_offset_z) / accel_scale;

    return sqrtf(ax_g*ax_g + ay_g*ay_g + az_g*az_g);
}

void MPU6050_resetCalibration(MPU6050* mpu) {
    mpu->accel_offset_x = 0;
    mpu->accel_offset_y = 0;
    mpu->accel_offset_z = 0;
    mpu->gyro_offset_x = 0;
    mpu->gyro_offset_y = 0;
    mpu->gyro_offset_z = 0;
    mpu->calibrated = 0;
}

// 获取加速度计量程对应的 LSB/g 比例因子
static float MPU6050_getAccelScale(MPU6050* mpu) {
    switch (mpu->accel_fs) {
        case MPU6050_ACCEL_FS_2G:  return 16384.0f;
        case MPU6050_ACCEL_FS_4G:  return 8192.0f;
        case MPU6050_ACCEL_FS_8G:  return 4096.0f;
        case MPU6050_ACCEL_FS_16G: return 2048.0f;
        default: return NAN;
    }
}

// 获取陀螺仪量程对应的 LSB/dps 比例因子
static float MPU6050_getGyroScale(MPU6050* mpu) {
    switch (mpu->gyro_fs) {
        case MPU6050_GYRO_FS_250DPS:  return 131.0f;
        case MPU6050_GYRO_FS_500DPS:  return 65.5f;
        case MPU6050_GYRO_FS_1000DPS: return 32.8f;
        case MPU6050_GYRO_FS_2000DPS: return 16.4f;
        default: return NAN;
    }
}
